%% 5.3节 中末制导联合仿真任务1 作图
clc
clear
load('LLA123mid.mat')
load('LLA123end.mat')
time1mid = (1:1:size(LLA1mid, 2))*0.1;
time2mid = (1:1:size(LLA2mid, 2))*0.1;
time3mid = (1:1:size(LLA3mid, 2))*0.1;
time1end = time1mid(end) + (1:1:size(LLA1end, 2))*0.01;
time2end = time2mid(end) + (1:1:size(LLA2end, 2))*0.01;
time3end = time3mid(end) + (1:1:size(LLA3end, 2))*0.01;

f = figure('Name', 'Trajectory');
plot3(LLA1mid(1,:), LLA1mid(2,:), LLA1mid(3,:), '-.', 'LineWidth',2);grid;hold on;
plot3(LLA2mid(1,:), LLA2mid(2,:), LLA2mid(3,:), '--', 'LineWidth',2);
plot3(LLA3mid(1,:), LLA3mid(2,:), LLA3mid(3,:), ':', 'LineWidth',2);
plot3(LLA1end(1,:), LLA1end(2,:), LLA1end(3,:), '-.', 'LineWidth',2);
plot3(LLA2end(1,:), LLA2end(2,:), LLA2end(3,:), '--', 'LineWidth',2);
plot3(LLA3end(1,:), LLA3end(2,:), LLA3end(3,:), ':', 'LineWidth',2);
plot3(50.5,50.5,0,'*','LineWidth',4);
xlabel('经度/°');ylabel('纬度/°');zlabel('高度/km');
legend('飞行器1','飞行器2','飞行器3');legend('boxoff');
saveas(f, f.Name, 'svg')

f = figure('Name', '地面轨迹');
plot(LLA1mid(1,:), LLA1mid(2,:), '-.', LLA2mid(1,:), LLA2mid(2,:), '--', LLA3mid(1,:), LLA3mid(2,:), ':', 'linewidth', 1.5);hold on;
plot(LLA1end(1,:), LLA1end(2,:), '-.', LLA2end(1,:), LLA2end(2,:), '--', LLA3end(1,:), LLA3end(2,:), ':', 'linewidth', 1.5);
xlabel('经度/°'); ylabel('纬度/°');
legend('飞行器1', '飞行器2', '飞行器3','Location','northwest');legend('boxoff');
a= 50;%圆心横坐标
b= 50;%圆心纵坐标
c= 2;%长半轴
d= 2;%短半轴
theta=0:0.1:2*pi;
x=a+c*cos(theta);
y=b+d*sin(theta);
plot(x,y,'k:','HandleVisibility','off','linewidth', 1.5);
annotation('arrow',[0.81 0.9],[0.8 0.8]);
saveas(f, f.Name, 'svg')

f = figure('Name', '地面轨迹的图中图');
plot(LLA1mid(1,:), LLA1mid(2,:), '-.', LLA2mid(1,:), LLA2mid(2,:), '--', LLA3mid(1,:), LLA3mid(2,:), ':', 'linewidth', 1.5);hold on;
plot(LLA1end(1,:), LLA1end(2,:), '-.', LLA2end(1,:), LLA2end(2,:), '--', LLA3end(1,:), LLA3end(2,:), ':', 'linewidth', 1.5);
xlim([49.8 50.6]); ylim([49.8 50.6]);
plot3(50.5,50.5,0,'*','LineWidth',3);
xlabel('经度/°'); ylabel('纬度/°');
legend('飞行器1（再入段）', '飞行器2（再入段）', '飞行器3（再入段）','飞行器1（末段）', '飞行器2（末段）', '飞行器3（末段）','Location','northwest');legend('boxoff');
saveas(f, f.Name, 'svg')

f = figure('Name', '高度-时间历程');
plot(time1mid, LLA1mid(3,:), ':', time2mid, LLA2mid(3,:), '-.', time3mid, LLA3mid(3,:),  '--', 'linewidth', 1.5);hold;
plot(time1end, LLA1end(3,:), ':', time2end, LLA2end(3,:), '-.', time3end, LLA3end(3,:), '--', 'linewidth', 1.5);
xlabel('时间/s'); ylabel('高度/km');
legend('飞行器1', '飞行器2', '飞行器3','Location','northeast');legend('boxoff');
a= 1450;%圆心横坐标
b= 28.5;%圆心纵坐标
c= 40;%长半轴
d= 2;%短半轴
theta=0:0.1:2*pi;
x=a+c*cos(theta);
y=b+d*sin(theta);
plot(x,y,'k:','HandleVisibility','off','linewidth', 1.5);
ax2 = axes('Position',[0.5 0.2 0.2 0.2]);
plot(time1mid, LLA1mid(3,:), ':', time2mid, LLA2mid(3,:), '-.', time3mid, LLA3mid(3,:),  '--', 'linewidth', 1.5);hold;
plot(time1end, LLA1end(3,:), ':', time2end, LLA2end(3,:), '-.', time3end, LLA3end(3,:), '--', 'linewidth', 1.5);
xlim([1430 1470])
ylim([27 29.5])
annotation('arrow',[0.8 0.7],[0.4 0.3]);
saveas(f, f.Name, 'svg')

f = figure('Name', '速度-时间历程');
plot(time1mid, V1mid, ':', time2mid, V2mid,'-.', time3mid, V3mid,  '--', 'linewidth', 1.5);hold;
plot(time1end, V1end, ':', time2end, V2end, '-.', time3end, V3end, '--', 'linewidth', 1.5);
xlabel('时间/s'); ylabel('速度/(m·s^{-1}）');
legend('飞行器1', '飞行器2', '飞行器3');legend('boxoff');
a= 1450;
b= 2010;
c= 40;
d= 200;
theta=0:0.1:2*pi;
x=a+c*cos(theta);
y=b+d*sin(theta);
plot(x,y,'k:','HandleVisibility','off','linewidth', 1.5);
annotation('arrow',[0.8 0.7],[0.35 0.3]);
ax2 = axes('Position',[0.4 0.2 0.2 0.2]);
plot(time1mid, V1mid, ':', time2mid, V2mid,'-.', time3mid, V3mid,  '--', 'linewidth', 1.5);hold;
plot(time1end, V1end, ':', time2end, V2end, '-.', time3end, V3end, '--', 'linewidth', 1.5);
xlim([1449 1453])
ylim([1980 2020])
saveas(f, f.Name, 'svg')

f = figure('Name', '弹道倾角-时间历程');
plot(time1mid, fpa1mid, ':', time2mid, fpa2mid,'-.', time3mid, fpa3mid,  '--', 'linewidth', 1.5);hold;
plot(time1end, fpa1end*180/pi, ':', time2end, fpa2end*180/pi, '-.', time3end, fpa3end*180/pi, '--', 'linewidth', 1.5);
xlabel('时间/s'); ylabel('弹道倾角/°');
legend('飞行器1', '飞行器2', '飞行器3', 'Location','southwest');legend('boxoff');
saveas(f, f.Name, 'svg')

f = figure('Name', '弹道偏角-时间历程');
plot(time1mid, azi1mid, ':', time2mid, azi2mid,'-.', time3mid, azi3mid,  '--', 'linewidth', 1.5);hold;
plot(time1end, azi1end*180/pi, ':', time2end, azi2end*180/pi, '-.', time3end, azi3end*180/pi, '--', 'linewidth', 1.5);
xlabel('时间/s'); ylabel('弹道偏角/°');
legend('飞行器1', '飞行器2', '飞行器3', 'Location','southwest');legend('boxoff');
saveas(f, f.Name, 'svg')